# servo_simulator.py
import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import Rectangle, Circle
import time


class VirtualServoSimulator:
    def __init__(self):
        """初始化虚拟舵机模拟器"""
        # 创建图形窗口
        self.fig, self.ax = plt.subplots(figsize=(8, 6))
        self.ax.set_xlim(-2, 2)
        self.ax.set_ylim(-2, 2)
        self.ax.set_aspect('equal')
        self.ax.set_title('🤖 奥特曼舵机模拟器 (虚拟模式)', fontsize=16, pad=20)

        # 绘制底座
        self.base = Rectangle((-0.8, -0.1), 1.6, 0.2, color='#3498db', alpha=0.8)
        self.ax.add_patch(self.base)

        # 绘制舵机臂（初始水平方向）
        self.arm, = self.ax.plot([0, 1], [0, 0], 'r-', linewidth=8, solid_capstyle='round')

        # 绘制连接点（舵机轴心）
        self.joint = Circle((0, 0), 0.1, color='#e74c3c')
        self.ax.add_patch(self.joint)

        # 显示当前角度
        self.angle_text = self.ax.text(0, -1.8, '当前角度: 90°', ha='center', fontsize=14, color='#2c3e50')

        # 添加操作说明
        self.instructions = self.ax.text(0, -1.5,
                                         "输入指令: 'hug', 'wave', 'reset'\n或直接输入角度 (0~180)",
                                         ha='center', fontsize=12, color='#7f8c8d')

        plt.ion()  # 开启交互模式
        plt.show(block=False)
        self.set_angle(90)  # 初始化为90度位置

    def set_angle(self, angle_degrees):
        """设置舵机角度并更新动画"""
        angle_rad = np.radians(angle_degrees)
        end_x = np.cos(angle_rad)  # 计算臂的终点坐标
        end_y = np.sin(angle_rad)

        # 更新臂的位置
        self.arm.set_data([0, end_x], [0, end_y])
        self.angle_text.set_text(f'当前角度: {angle_degrees}°')

        # 刷新画面
        self.fig.canvas.draw()
        self.fig.canvas.flush_events()
        time.sleep(0.05)  # 微小延迟让动画更平滑

    def perform_hug(self):
        """模拟拥抱动作序列"""
        print("\n=== 执行拥抱动作 ===")
        for angle in [90, 180, 90, 0]:  # 从中间→张开→中间→合拢
            self.set_angle(angle)
            time.sleep(0.5)
        print("✅ 拥抱动作完成！")

    def perform_wave(self):
        """模拟挥手动作序列"""
        print("\n=== 执行挥手动作 ===")
        for _ in range(3):  # 快速摆动3次
            self.set_angle(45)
            time.sleep(0.3)
            self.set_angle(135)
            time.sleep(0.3)
        self.set_angle(90)  # 复位
        print("✅ 挥手动作完成！")

    def reset(self):
        """复位到初始位置"""
        print("\n=== 复位舵机 ===")
        self.set_angle(90)
        print("✅ 已复位到90°位置")


def main():
    """主测试函数"""
    print("=== 虚拟舵机模拟器启动 ===")
    print("可输入以下指令测试：")
    print("  'hug'    - 模拟拥抱动作")
    print("  'wave'   - 模拟挥手动作")
    print("  'reset'  - 复位到90度")
    print("  '45'     - 直接转到45度")
    print("  'exit'   - 退出程序\n")

    simulator = VirtualServoSimulator()

    try:
        while True:
            cmd = input("请输入指令: ").strip().lower()
            if cmd == 'hug':
                simulator.perform_hug()
            elif cmd == 'wave':
                simulator.perform_wave()
            elif cmd == 'reset':
                simulator.reset()
            elif cmd == 'exit':
                break
            elif cmd.isdigit():
                angle = int(cmd)
                if 0 <= angle <= 180:
                    print(f"转到 {angle}°")
                    simulator.set_angle(angle)
                else:
                    print("⚠️ 角度必须在0~180之间！")
            else:
                print("⚠️ 未知指令！请输入 'hug', 'wave', 'reset' 或角度值")

    except KeyboardInterrupt:
        print("\n检测到中断，退出程序...")
    finally:
        simulator.reset()
        plt.close()
        print("=== 模拟器已关闭 ===")


if __name__ == "__main__":
    main()